#include "path_planning/pixel.h"


PixelPoint* RealCoordToPixel(double x, double y, double resolution, unsigned int width, unsigned int height){
	double x_broken = (x < 0) ? width/2.0 + x/resolution : width/2.0 + x/resolution;
	double y_broken = (y < 0) ? height/2.0 + y/resolution : height/2.0 + y/resolution;

	PixelPoint* point = new PixelPoint();
	point->x = (x_broken - (int)x_broken)*10 < 5 ? (int)x_broken : (int)x_broken+1;
	point->y = (y_broken - (int)y_broken)*10 < 5 ? (int)y_broken : (int)y_broken+1;

	return point;
}
